#include "MapNode.h"

int main(int argc, char **argv)
{

	ros::init(argc, argv, "mapNode");
	ros::NodeHandle n;
	ros::Subscriber state_sub = n.subscribe("State/Info", 1, state_update);
	ros::Subscriber enc_sub = n.subscribe("/serial/encoder", 1, recive_encoder);//when "/encoder" topic is revieved call recive_encoder function
	ros::Subscriber ir_sub = n.subscribe("ir_info", 1, recieve_ir);
	vis_pub = n.advertise<visualization_msgs::Marker>("/visualization_marker", 1 );
	map_pub = n.advertise<nav_msgs::OccupancyGrid>("/grid",1);
	rob_pos_pub = n.advertise<visualization_msgs::Marker>("/visualization_pos",1);
	pos_pub = n.advertise<saint_nr_9::RobotPosition>("/position", 1);
  ros::Subscriber sub_path = n.subscribe("Signals/map_path", 1, initialize_map);
	ros::Rate loop_rate(100);
  
  mat = new map_type*[ROWS];
  for(int i = 0; i < ROWS; i++) {
    mat[i] = new map_type[COLS];
    memset(mat[i], MAP_UNEXPLORED, COLS);
  }
  int count = 0;
  
	while(ros::ok())
	{
	    int old_state = global_state;
	    
	    right_wall_dist = (ir_data.white + ir_data.orange)/2; // Approximate distance to wall if going straight
	    right_wall_dist += HALF_ROBO_WIDTH;
	    left_wall_dist = ir_data.black; //  + ir_data.green
	    left_wall_dist += HALF_ROBO_WIDTH;
	    
	    if (encoder_subscription_started)
	    {
	      switch (global_state)
	      {
          case STRAIGHT_WALL:       // Move straight
            straight_odo_calc(true);
            wall_adjust(true);
            sensor_calc();
            calc_and_paint_walls_and_free_space();
            break;
            
          case PART_HOLE_FRONT:     // Move straight
            straight_odo_calc(true);
            sensor_calc();
            calc_and_paint_walls_and_free_space();
            break;
            
          case PART_WALL_FRONT:     // Move straight
            straight_odo_calc(true);
            sensor_calc();
            calc_and_paint_walls_and_free_space();
            break;
            
          case PART_WALL_PART_HOLE: // Move straight
            straight_odo_calc(true);
            sensor_calc();
            calc_and_paint_walls_and_free_space();
            break;
            
          case FIND_WALL:           // Move straight
            straight_odo_calc(true);
            sensor_calc();
            calc_and_paint_walls_and_free_space();
    	      break;
    	      
	        case CORNER:              // Turn left 90

	          global_state = -1;
	          end_right_wall = current_wall_pos;
	          end_right_wall.x += HALF_ROBO_WIDTH*move.x;
	          end_right_wall.y += HALF_ROBO_WIDTH*move.y;
	          draw_straight_line(start_right_wall, end_right_wall, MAP_WALL, 1);

            build_wall_segments(left_wall_points, left_wall_points_robo, move);
            left_wall_points.clear();

 	          new_right_wall = true;
	          move = calculate_direction(move, false);
	          break;
	          
	        case HOLE:                // Turn right 90
	          global_state = -1;
	          end_right_wall = current_wall_pos;
    	      new_right_wall = true;

            build_wall_segments(left_wall_points, left_wall_points_robo, move);
            left_wall_points.clear();

    	      draw_straight_line(start_right_wall, end_right_wall, MAP_WALL, 1);
	          move = calculate_direction(move, true);
	          break;
	          
          case WOWOWO:              // Move straight backwards
            straight_odo_calc(false);
            break;
            
          case WOWOWO_TURN:         // Turn left
            global_state = -1;
            move = calculate_direction(move, false);
            break;
	          
	        case TAG_HANDLE:          // Move backwards
	          global_state = -2; // Helper state
	          
            straight_odo_calc(false);
            sensor_calc();
            calc_and_paint_walls_and_free_space();
            
	          tag_list.push_back(robo_pos);
	          create_marker(robo_pos.x, robo_pos.y, tag_list.size());
	          printf("sent marker\n");
	          break;
	          
	        case -2: // Move straight backwards.
	                  // Tag handle helper, to only send/store tag position once.
            straight_odo_calc(false);
            sensor_calc();
            calc_and_paint_walls_and_free_space();
	          break;

          case TAG_PROCESSING:      // Stand still
            break;
            
          case START:
            break;
            
          case FIND_FIRST_WALL:     // Move straight
            straight_odo_calc(true);
            wall_adjust(false);
            break;
            
          case PATH_STRAIGHT:       // Move straight
            straight_odo_calc(true);
            wall_adjust(false);
            break;
            
          case PATH_LEFT_TURN:      // Turn left 90
	          global_state = -1;
	          move = calculate_direction(move, false);
            break;
            
          case PATH_RIGHT_TURN:     // Turn right 90
	          global_state = -1;
	          move = calculate_direction(move, true);
            break;
            
          case PATH_U_TURN:         // Turn 180 "somehow"
	          move = calculate_direction(move, true);
	          move = calculate_direction(move, true);
	          global_state = -1;
            break;
            
          case PATH_SEARCH_TAG:     // ???
            break;
            
          case WOWOWO_PATH:         // Move straight backwards
            straight_odo_calc(false);
            break;
	        default:
	          break;
	          
	      }
	      
	      
	      if (count > 500) { 
          publish_grid();
	        send_position();	      
	        count = count % 500; 
	      }
	     
	      robo_pos_msg.x = robo_pos.x;
	      robo_pos_msg.y = robo_pos.y;
	      robo_pos_msg.heading = POS_DOWN;
	      pos_pub.publish(robo_pos_msg);
	      count++;
	      loop_rate.sleep();
			  ros::spinOnce();
			  // Maybe not needed anymore
			  if( global_state != old_state)// && (old_state == CORNER || old_state == HOLE ))
	      {
	        sl = l;
	        sr = r;
	      }
	    }
	    else
	    {
	      robo_pos_msg.x = robo_pos.x;
	      robo_pos_msg.y = robo_pos.y;
	      if (equal_direction(move, up)) {
	        robo_pos_msg.heading = POS_UP;
	      }else if (equal_direction(move, down)) {
	        robo_pos_msg.heading = POS_DOWN;
	      }else if (equal_direction(move, left)) {
	        robo_pos_msg.heading = POS_LEFT;
	      }else if (equal_direction(move, right)) {
	        robo_pos_msg.heading = POS_RIGHT;
	      }
	      pos_pub.publish(robo_pos_msg);
	      loop_rate.sleep();
			  ros::spinOnce();
			}
	  }

	save_map(MAP_PATH);
	save_map_real(MAP_PATH);

	for (int r = 0; r < ROWS; r++) {
	  delete [] mat[r];
	}
	delete [] mat;
	
	//publish_grid();
	return 0;
}

void sensor_calc()
{
  direction right_wall_dir = calculate_direction(move, true);
  direction left_wall_dir = calculate_direction(move, false);
  
  if (ir_data.white > RIGHT_WALL_FAR) {
    right_front = RIGHT_WALL_FAR;
    right_front_far = true;
  }else{
    right_front = round(ir_data.white);
    right_front_far = false;
  }
  
  if (ir_data.orange > RIGHT_WALL_FAR) {
    right_back = RIGHT_WALL_FAR;
    right_back_far = true;
  }else{
    right_back = round(ir_data.orange);
    right_back_far = false;
  }
  
  if (ir_data.green > LEFT_WALL_FAR) {
    left_front = LEFT_WALL_FAR;
    left_front_far = true;
  }else{
    left_front = round(ir_data.green);
    left_front_far = false;
  }
  
  if (ir_data.black > LEFT_WALL_FAR) {
    left_back = LEFT_WALL_FAR;
    left_back_far = true;
  }else{
    left_back = round(ir_data.black);
    left_back_far = false;
  }
  
  pos_left_front.x = robo_pos.x + HALF_ROBO_WIDTH * move.x + (HALF_ROBO_WIDTH + left_front) * left_wall_dir.x;
  pos_left_front.y = robo_pos.y + HALF_ROBO_WIDTH * move.y + (HALF_ROBO_WIDTH + left_front) * left_wall_dir.y;
  pos_left_back.x = robo_pos.x - HALF_ROBO_WIDTH * move.x + (HALF_ROBO_WIDTH + left_back) * left_wall_dir.x;
  pos_left_back.y = robo_pos.y - HALF_ROBO_WIDTH * move.y + (HALF_ROBO_WIDTH + left_back) * left_wall_dir.y;
  pos_right_front.x = robo_pos.x + HALF_ROBO_WIDTH * move.x + (HALF_ROBO_WIDTH + right_front) * right_wall_dir.x;
  pos_right_front.y = robo_pos.y + HALF_ROBO_WIDTH * move.y + (HALF_ROBO_WIDTH + right_front) * right_wall_dir.y;
  pos_right_back.x = robo_pos.x - HALF_ROBO_WIDTH * move.x + (HALF_ROBO_WIDTH + right_back) * right_wall_dir.x;
  pos_right_back.y = robo_pos.y - HALF_ROBO_WIDTH * move.y + (HALF_ROBO_WIDTH + right_back) * right_wall_dir.y;
}

void initialize_map(const std_msgs::String::ConstPtr& signal)
{
  printf("Hui a map :) \n");
  readMap(signal->data);
}

void state_update(const std_msgs::Int32::ConstPtr& state) {
  global_state = state->data;
  printf("Input state: %d\n", state->data);
  sl = l;
	sr = r;
  //state_functions[state->data]();
}

bool equal_direction(direction one, direction two)
{
  return (one.x == two.x && one.y == two.y);
}

direction calculate_direction(direction old_direction, bool turn_right)
{
  if (turn_right)
  {
    if (equal_direction(down, old_direction)) return left;
    if (equal_direction(left, old_direction)) return up;
    if (equal_direction(up, old_direction)) return right;
    if (equal_direction(right, old_direction)) return down;
  }
  else
  {
    if (equal_direction(down, old_direction)) return right;
    if (equal_direction(left, old_direction)) return down;
    if (equal_direction(up, old_direction)) return left;
    if (equal_direction(right, old_direction)) return up;
  }
}

void publish_grid()
{
  nav_msgs::OccupancyGrid msg;

  msg.header.frame_id = "/map";
  msg.header.stamp = ros::Time::now();

  msg.info.map_load_time = ros::Time::now();
  msg.info.resolution = 0.01;
  msg.info.width = 1000;
  msg.info.height = 1000;


  msg.info.origin.position.x = 0;//(float)-1000/200; 
  msg.info.origin.position.y = 0;//(float)-1000/200; 
  msg.info.origin.position.z = 0;

  msg.info.origin.orientation.x = 0;
  msg.info.origin.orientation.y = 0;
  msg.info.origin.orientation.z = 0; //sin(robotPose_theta/2);
  msg.info.origin.orientation.w = 1; //cos(robotPose_theta/2);

  msg.data.resize(ROWS*COLS);

  for (unsigned int i=0; i<ROWS; i++) {
    for (unsigned int j=0; j<COLS; j++) {
      int set_value = 0;
      if (mat[j][i] == MAP_UNEXPLORED) set_value = 50;
      else if (mat[j][i] == MAP_FREE) set_value = 0;
      else if (mat[j][i] == MAP_WALL) set_value = 100;
      msg.data[i*1000+j] = set_value;
    }
  }
  
  
  map_pub.publish(msg);
  
  /*
  for(int i = 150; i < 250; i++){
    for(int y = 150; y < 250; y++){
      if(mat[i][y])
        printf("1");
       else
        printf("0");
    }
    printf("\n");
  }
  
  printf("\n");*/
  
}

void recive_encoder(const Encoder::ConstPtr &msg){
	r = msg->right;
	l = msg->left;
	if (!encoder_subscription_started)
	{
	  sr = r;
	  sl = l;
	}
	encoder_subscription_started = true;
	//printf("%f:got encoder L:%i , false R:%i\n",msg->timestamp,l,r);
}

void recieve_ir(const saint_nr_9::IRdata::ConstPtr &data){
  ir_data = *data;
}

int dist(int a , int b){
	if(a >= 0 && b >= 0)
		return abs(a-b);
	if(a >= 0 && b < 0)
		return abs(a-b);
	if(a <= 0 && b < 0)
		return abs(a-b);
	if(a <= 0 && b >= 0)
		return abs(a)+b;
		
	return -1; //How the hell did we get here???
}

void send_position()
{
  visualization_msgs::Marker marker;
  marker.header.frame_id = "/map";
  marker.header.stamp = ros::Time();
  marker.ns = "my_namespace";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::CUBE;
  marker.action = visualization_msgs::Marker::ADD;
  marker.lifetime = ros::Duration();
  //marker.pose.position.x = pos_x_actual*0.01;
  //marker.pose.position.y = pos_y_actual*0.01;
  marker.pose.position.x = robo_pos.x*0.01;
  marker.pose.position.y = robo_pos.y*0.01;
  marker.pose.position.z = 0;
  marker.pose.orientation.x = 0.0;
  marker.pose.orientation.y = 0.0;
  marker.pose.orientation.z = 0.0;
  marker.pose.orientation.w = 1.0;
  marker.scale.x = 0.05;
  marker.scale.y = 0.05;
  marker.scale.z = 0.05;
  marker.color.a = 1;
  marker.color.r = 1.0;
  marker.color.g = 0.0;
  marker.color.b = 0.0;
  
 /* for (int i = 0; i < tag_list.size();i++)
  {
    geometry_msgs::Point p;
    p.x = tag_list[i].x;
    p.y = tag_list[i].y;
    p.z = 10;
    marker.points.push_back(p);
  }*/
  //only if using a MESH_RESOURCE marker type:
  //marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
  rob_pos_pub.publish(marker);
  ros::spinOnce();

}

void straight_odo_calc(bool forward)
{
  prev_robo_pos = robo_pos;
  int dir_mod;
  if (forward) { dir_mod = 1; } else { dir_mod = -1; }
  robo_pos.x += round(dir_mod*move.x*(dist(r, sr) + dist(l, sl))/16);
  robo_pos.y += round(dir_mod*move.y*(dist(r, sr) + dist(l, sl))/16);
  sl = l;
  sr = r;
}

void draw_straight_line(const position& start, const position& end, map_type type, int thickness) {
  
  position d_start, d_end;
  int half_t = thickness/2;
  
  if (start.x == end.x)
  {
    if (start.y < end.y)
    {
      d_start = start;
      d_end = end;
    }else{
      d_start = end;
      d_end = start;
    }
    for (int y = max(d_start.y, 0); y < min(d_end.y, COLS); y++) {
      for (int x = max(d_start.x - half_t-1, 0); x < min(d_start.x + half_t, ROWS);x++) {
        if (mat[x][y] != MAP_WALL) {
          mat[x][y] = type;
        }
      }
    }

  }
  else
  {
    if (start.x < end.x)
    {
      d_start = start;
      d_end = end;
    }else{
      d_start = end;
      d_end = start;
    }
    for (int x = max(d_start.x, 0); x < min(d_end.x, ROWS); x++) {
      for (int y = max(d_start.y - half_t-1, 0); y < min(d_start.y + half_t, COLS); y++) {
        if (mat[x][y] != MAP_WALL) {
          mat[x][y] = type;
        }
      }
    }
  }
}

int median(const std::vector<int>& v)
{
  if (v.size() == 0) throw std::out_of_range("sarah");
  std::vector<int> copy = v;
  std::sort(copy.begin(), copy.end());
  return  copy[copy.size()/2];
}

void build_wall_segments(const std::vector<position>& left_wall_points, const std::vector<position>& robo_points, const direction& move)
{
  if (left_wall_points.empty()) return;
  
  position next_point;
  position next_robo_point;
  std::vector<int> segment;
  std::vector<position> segment_robo;
  direction direc_wall = calculate_direction(move, true);
  
  if (move.x != 0) {
    segment.push_back(left_wall_points[0].y);
    segment_robo.push_back(robo_points[0]);
  }else{
    segment.push_back(left_wall_points[0].x);
    segment_robo.push_back(robo_points[0]);
  }
  int seg_start_index = 0;
  int seg_end_index = 0;
  
  for (size_t cur_index = 0; cur_index < left_wall_points.size(); cur_index++) {
    if (cur_index != left_wall_points.size() - 1) {
      next_point = left_wall_points[cur_index+1];
      next_robo_point = robo_points[cur_index+1];
    }else{
      next_point.x = INT_MAX;
      next_point.y = INT_MAX;
    }
    
    if (move.x != 0) {
    
      if (dist(next_point.y, median(segment)) <3) {
        segment.push_back(next_point.y);
        segment_robo.push_back(next_robo_point);
        seg_end_index = cur_index;
      }else{
        position from, to;
        int med = median(segment);
        from.x = left_wall_points[seg_start_index].x;
        from.y = med;
        to.x = left_wall_points[seg_end_index].x;
        to.y = med;
        draw_straight_line(from, to, MAP_WALL, 1);
        
        segment.clear();
        segment_robo.clear();
        if (cur_index != left_wall_points.size()-1) {
          segment.push_back(next_point.y);
          segment_robo.push_back(next_robo_point);
          seg_start_index = cur_index + 1;
        }
      }
      
    }else{
    
      if (dist(next_point.x, median(segment)) <3) {
        segment.push_back(next_point.x);
        seg_end_index = cur_index;
      }else{
        position from, to;
        from.x = median(segment);
        from.y = left_wall_points[seg_start_index].y;
        to.x = median(segment);
        to.y = left_wall_points[seg_end_index].y;
        draw_straight_line(from, to, MAP_WALL, 1);
        segment.clear();
        if (cur_index != left_wall_points.size()-1) {
          segment.push_back(next_point.x);
          seg_start_index = cur_index + 1;
        }
      }
    
    }
  }
}

bool find_closest_wall(const position& pos, const direction& dir, position& wall_pos, int& distance)
{
  position cur_pos = pos;
  distance = 0;
  while (cur_pos.x < ROWS && cur_pos.x >= 0 && cur_pos.y < COLS && cur_pos.y >= 0) {
    if (mat[cur_pos.x][cur_pos.y] == MAP_WALL) {
      wall_pos.x = cur_pos.x;
      wall_pos.y = cur_pos.y;
      return true;
    }else if (mat[cur_pos.x][cur_pos.y] == MAP_FREE) {
      // Just let it go.
    }else{
      return false;
    }
    cur_pos.x += dir.x;
    cur_pos.y += dir.y;
    distance++;
  }
  return false;
}

void wall_adjust(bool following_right)
{
  direction direc_wall = calculate_direction(move, true);
  
  bool existing_wall_in_map = false;
  position existing_wall;
  int dist_to_found_wall = 0;
  
  // TODO: Robo pos or front sensor pos!? (front of course)
  existing_wall_in_map = find_closest_wall(
    position(robo_pos.x+move.x, robo_pos.y+move.y),
    direc_wall, existing_wall, dist_to_found_wall);
  
  if (existing_wall_in_map && dist(dist_to_found_wall, right_wall_dist) < 15) // TODO: Add direction
  {
    robo_pos.x = existing_wall.x - move.x + round(right_wall_dist) * direc_wall.x*(-1);
    robo_pos.y = existing_wall.y - move.y + round(right_wall_dist) * direc_wall.y*(-1);
  }
  else if (new_right_wall) // TODO: include if following right otherwise adjust for either left or right in that order
  {
    new_right_wall = false;
    // Set the wall pos when beginning to follow a new wall
    current_wall_pos.x = robo_pos.x + round(right_wall_dist) * direc_wall.x;
    current_wall_pos.y = robo_pos.y + round(right_wall_dist) * direc_wall.y;
    start_right_wall = current_wall_pos;
    start_right_wall.x += - HALF_ROBO_WIDTH*move.x;
    start_right_wall.y += - HALF_ROBO_WIDTH*move.y;
    //printf("wall (x,y) = (%d,%d)\n robot (x,y) = (%d,%d)\n", current_wall_pos.x, current_wall_pos.y, robo_pos.x, robo_pos.y);
    //printf("ir_white = %f\n\n", ir_data.white);
  }
  else
  {
    // Set robo pos according to the distance to the wall
    if (move.x != 0)
    {
      robo_pos.y = current_wall_pos.y + round(right_wall_dist) * direc_wall.y * (-1);
      current_wall_pos.x = robo_pos.x;
    }
    else
    {
      robo_pos.x = current_wall_pos.x + round(right_wall_dist) * direc_wall.x * (-1);
      current_wall_pos.y = robo_pos.y;
    }
  }
}

void fill_area(const position& pos1, const position& pos2, map_type value, bool overwrite_walls) {
  int start_x, end_x, start_y, end_y;
  
  if (pos1.x < pos2.x) {
    start_x = pos1.x;
    end_x = pos2.x;
  }else{
    start_x = pos2.x;
    end_x = pos1.x;
  }
  
  if (pos1.y < pos2.y) {
    start_y = pos1.y;
    end_y = pos2.y;
  }else{
    start_y = pos2.y;
    end_y = pos1.y;
  }
  //printf(" X (%d to %d)  Y (%d to %d) \n", start_x, end_x, start_y, end_y);
  for (int x = start_x; x <= end_x; x++) {
    for (int y = start_y; y <= end_y; y++) {
      if (x < ROWS && x >= 0 && y < COLS && y >= 0) {
        if (mat[x][y] != MAP_WALL || overwrite_walls) {
          mat[x][y] = value;
        }
      }
    }
  }
}

void calc_and_paint_walls_and_free_space()
{
  direction direc_wall = calculate_direction(move, true);

  if (move.x != 0)
  {
  
    // Right
    // Front
    fill_area(
      position(prev_robo_pos.x+HALF_ROBO_WIDTH*move.x, prev_robo_pos.y),
      pos_right_front,
      MAP_FREE, false);
    // Back
    fill_area(
      position(prev_robo_pos.x-HALF_ROBO_WIDTH*move.x, prev_robo_pos.y),
      pos_right_back,
      MAP_FREE, false);
    
    // Left
    // Front
    fill_area(
      position(prev_robo_pos.x+HALF_ROBO_WIDTH*move.x, prev_robo_pos.y),
      pos_left_front,
      MAP_FREE, false);
    fill_area(
      position(prev_robo_pos.x-HALF_ROBO_WIDTH*move.x, prev_robo_pos.y),
      pos_left_back,
      MAP_FREE, false);
    
    if (left_wall_dist < LEFT_WALL_FAR) {
      position left_wall_pos;
      left_wall_pos.y = robo_pos.y + round(left_wall_dist)*direc_wall.y*(-1);
      left_wall_pos.x = robo_pos.x + HALF_ROBO_WIDTH*move.x*(-1);

      //mat[left_wall_pos.x][left_wall_pos.y] = MAP_WALL;
      
      left_wall_points.push_back(left_wall_pos);
      left_wall_points_robo.push_back(robo_pos);
    }else{
      
      build_wall_segments(left_wall_points, left_wall_points_robo, move);      
      
      left_wall_points.clear();
      left_wall_points_robo.clear();
    }
    
  }else{
  
    // Right
    
    // Front
    fill_area(
      position(prev_robo_pos.x, prev_robo_pos.y+HALF_ROBO_WIDTH*move.y),
      pos_right_front,
      MAP_FREE, false);
    // Back
    fill_area(
      position(prev_robo_pos.x, prev_robo_pos.y-HALF_ROBO_WIDTH*move.y),
      pos_right_back,
      MAP_FREE, false);    
    
    // Left
    
    // Front
    fill_area(
      position(prev_robo_pos.x, prev_robo_pos.y+HALF_ROBO_WIDTH*move.y),
      pos_left_front,
      MAP_FREE, false);
      
    // Back
    fill_area(
      position(prev_robo_pos.x, prev_robo_pos.y-HALF_ROBO_WIDTH*move.y),
      pos_left_back,
      MAP_FREE, false);
      
    if (left_wall_dist < LEFT_WALL_FAR) {
      position left_wall_pos;
      left_wall_pos.x = robo_pos.x + round(left_wall_dist)*direc_wall.x*(-1);
      left_wall_pos.y = robo_pos.y + HALF_ROBO_WIDTH*move.y*(-1);

      //mat[left_wall_pos.x][left_wall_pos.y] = MAP_WALL;

      left_wall_points.push_back(left_wall_pos);
      left_wall_points_robo.push_back(robo_pos);
    }else{
      
      build_wall_segments(left_wall_points, left_wall_points_robo, move);
      left_wall_points.clear();
      left_wall_points_robo.clear();
    }
    
  }
}

void create_marker(int xM, int yM, int id)
{
	visualization_msgs::Marker marker;
  marker.header.frame_id = "/map";
  marker.header.stamp = ros::Time();
  marker.ns = "my_namespace";
  marker.id = id;
  marker.type = visualization_msgs::Marker::CUBE;
  marker.action = visualization_msgs::Marker::ADD;
  marker.lifetime = ros::Duration();
  marker.pose.position.x = xM*0.01;
  marker.pose.position.y = yM*0.01;
  marker.pose.position.z = 0;
  marker.pose.orientation.x = 0.0;
  marker.pose.orientation.y = 0.0;
  marker.pose.orientation.z = 0.0;
  marker.pose.orientation.w = 1.0;
  marker.scale.x = 0.03;
  marker.scale.y = 0.03;
  marker.scale.z = 0.03;
  marker.color.a = 1;
  marker.color.r = 0.0;
  marker.color.g = 1.0;
  marker.color.b = 0.0;
  
 /* for (int i = 0; i < tag_list.size();i++)
  {
    geometry_msgs::Point p;
    p.x = tag_list[i].x;
    p.y = tag_list[i].y;
    p.z = 10;
    marker.points.push_back(p);
  }*/
  //only if using a MESH_RESOURCE marker type:
  //marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
  vis_pub.publish(marker);
  ros::spinOnce();
}

void save_map(string path)
{
  std::ofstream myfile (path.c_str());
  if (myfile.is_open())
  {
    for (int x = 0; x < ROWS; x++ )
    {
      for (int y = 0; y < COLS; y++ )
      {
        bool placed = false;
        for (int i = max(x-HALF_ROBO_WIDTH,0); i < min(x+HALF_ROBO_WIDTH,ROWS) && !placed; i++ )
        {
            for (int j = max(y-HALF_ROBO_WIDTH,0); j < min(y+HALF_ROBO_WIDTH,COLS) && !placed; j++ )
            {
              if (mat[i][j] == MAP_WALL)
              {
                myfile << WALL;
                placed = true;
              }    
            }
        }
        if( !placed)
        {
          if (mat[x][y] == MAP_FREE)
          {
            myfile << FREE_SPACE;
          } 
          else
          {
            myfile << UNKNOWN; 
          }
        }        
      }
      myfile << endl;
    }
    myfile.close();
  }
}

void save_map_real(string path)
{
  stringstream myPath;
  myPath <<"real_"<<path;
  std::ofstream myfile (myPath.str().c_str());
  if (myfile.is_open())
  {
    for (int x = 0; x < ROWS; x++ )
    {
      for (int y = 0; y < COLS; y++ )
      {
        myfile << mat[x][y];        
      }
      myfile << endl;
    }
    myfile.close();
  }
}


void readMap(string path)
{
  string line;
  stringstream myPath;
  myPath <<"real_"<<path;
  std::ifstream myfile (myPath.str().c_str());
  
    if (myfile.is_open())
    { 
      //printf("Hui map file opened :) \n");
      //string ha = "haha";
      //cout << ha.compare("haha")<<endl;
      int x = 0;
      while ( myfile.good() )
      {
        //printf("Hui map file opened :) \n");
        std::getline (myfile,line);
        //cout << line << " " << "Wallcompare: " << line.compare("WALLS") << "\n" <<endl;
        //line = "WALLS";
        //cout << line.compare("WALLS")<<endl;
        
        if (line.compare(0,5,"WALLS")==0)
        {
          //printf("Juhu \n");
          break;
        }
        for (int i = 0; i < (int)line.size();i++)
        {
          mat[x][i] = line[i];
        }
        x++;
      }
      myfile.close();
    }   
}

/*
int dist(int a , int b){
	if(a >= 0 && b >= 0)
		return abs(a-b);
	if(a >= 0 && b < 0)
		return abs(a-b);
	if(a <= 0 && b < 0)
		return abs(a-b);
	if(a <= 0 && b >= 0)
		return abs(a)+b;
		
	return -1; //How the hell did we get here???
}*/
